/*
 * RangeFinder.c
 *
 *  Created on: 29/12/2012
 *      Author: Bowmer
 */

#include "inc/hw_types.h"
#include "inc/lm4f120h5qr.h"
#include "driverlib/gpio.h"
#include "Servos.h"
#include "driverlib/sysctl.h"



extern volatile unsigned long rangeStart, rangeEnd;
extern volatile unsigned int rangeValid;
extern volatile unsigned int debug;


float findRange(void){

	float range;
	unsigned long count;

	range = 1234;
	/* The range measurement should only be considered valid if it's under 25 metres */
	while(range>25){
		/* Enable Timer A */
		WTIMER1_CTL_R |= 0x00000001;

		/* Start the trigger sequence. Ended in TIMER1_A_INT vector routine */
		GPIO_PORTC_DATA_R |= GPIO_PIN_6;

		rangeValid = 0;
		while(rangeValid == 0 && debug == 0){}


		// time = #ticks/F_CPU = #ticks / 40,000,000
		// Range = time * speed of sound / 2 = time * 343 / 2
		// therefore range(m) = #ticks/40,000,000 * 343 / 2 = #ticks * 0.0000042875
		count = rangeEnd - rangeStart;
		range = count * 0.0000042875;
	}

		return range;
}


float findRangeAtZero(void){
	float zeroRange;
	/* Disable auto servo update - by turning off the timer */
	WTIMER2_CTL_R &= ~(0x00000001);

	// Move the servos to zero degrees
	updateServos(0,0);

	// Wait for the servos to actually get there
	SysCtlDelay(30000000);

	// start the range finder sequence
	zeroRange =  findRange();

	/* Reenable auto servo update */
	WTIMER2_CTL_R |= 0x00000001;

	return zeroRange;

}
